#include "pid.h"



void pid_init(struct PID * pid, float p, float i, float d)
{	
	pid->err				= 0.0;	
	pid->integral		= 0.0;
	pid->err_last		= 0.0;
	pid->err_next 		= 0.0;
	pid->output			= 0.0;
	
	pid->Kp				= p;
	pid->Ki				= i;
	pid->Kd				= d;
}

#if 1
float pid_update(struct PID * pid, float set, float actual)
{
	pid->err = set - actual;
	
	pid->integral += pid->err;
	
	pid->output = pid->Kp*pid->err + pid->Ki*pid->integral + pid->Kd*(pid->err - pid->err_last);
	
	pid->err_last = pid->err;
	
	return pid->output;
}

#else

float pid_update(struct PID * pid, float set, float actual)
{
	float increment;
	
	pid->err = set - actual;
	
	increment = pid->Kp*(pid->err - pid->err_next) + pid->Ki*pid->err + pid->Kd*(pid->err - 2*pid->err_next + pid->err_last);
	
	pid->output += increment;
	
	pid->err_last = pid->err_next;
	
	pid->err_next = pid->err;
	
	return pid->output;
}

#endif




